#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@Copyright    :   Jihua Lab 2021.
@File    :   gripper_controller.py
@Author  :   WANG Hao 
@Contact :   wanghao@jihualab.com、haowanghk@gmail.com(Github)
@License :   (C)Copyright 2020-2021, JHL
@Version :   1.1
@Create Time    :   CURRENT_YEAR/CURRENT_MONTH/CURRENT_DATE CURRENT_HOUR:CURRENT_MINUTE:CURRENT_SECOND
@Modify Time      @Author    @Version    @Desciption
-------------------        --- -------    ---  --------   --- -----------
@File Description :   None 
'''

# here put the import lib

# the import lib
import logging
import threading
import subprocess

import uservo
import serial
import time

# import robot motion control 

from MotionGroup import MotionGroup
from ServoPose import Pose
from wrist_gripper import WristGripperControl
from robot_arm import ArmMotionControl

# inport ros py
import rospy  
from sensor_msgs.msg import JointState
from joint_state_exchange.msg import RobotState
from dual_arm_base_control.msg import TelemetryInfo
from dual_arm_base_control.srv import CtrlModeSelect
from std_msgs.msg import String


# flag
exitFlag = 0  # threading control flag
VERBOSE = False #rospr loginfo flag
G_CONTROL_MODE = 3  # control mode server 0,1,2,3 default is landing mode

HOST_STATE = True # the homer host ip port state , default is true

SHORT_TIME_DELAY = 0.02  # time delay para
MEDIA_TIME_DELAY = 0.10
LONG_TIME_DELAY = 1.0

# rad2deg: deg = RAD2DEG * a 
RAD2DEG = 57.298

# publisher
SAMPLING_FREQUENCY = 30  # units: Hz
fileName = "robot_arm_records.txt"
POSE_NUM = rospy.get_param('/teach_mode/way_point_num')  # default pose number is 300

# PARAMS
ending_servo_idx = [0, 1, 2, 3]

VEL = {'Very_low_spped': 10, 'LOW_SPEED': 15, 'MID_SPEED': 45, 'HIGH_SPEED': 60}

DUAL_ARM_INIT_POSE = [140.0, 50.0, 70.0, 0, 
                        2.0, -76.5, 65.0, 0, 40, 40]
# 横幅抓取初始位置,绝对角度模式
DUAL_ARM_START_POSE = [140.0, 50.0, -30.0, 0,
                      2, -82, -35, 0, 10, 10]
# 抓取目标位置
DUAL_ARM_TARGET_POSE = [140.0, 50.0, 80.0, 0,
                        2, -82, -35, 0, 80, 90]


# dual_arm class obj init
dual_arm = ArmMotionControl(arm_name="dual_arm", 
                            dof=rospy.get_param('/arm_info/dual_arm_dof'), 
                            CONTROL_MODE="DAMPING", 
                            with_wrist=False, 
                            is_debug=True,
                            damping_power=1000)
wrist_gripper = WristGripperControl(servo_channel=ending_servo_idx, is_debug=True)

# 初始化舵机管理器类对象、串口端口地址
dual_arm_obj = dual_arm.creat_uservo_obj(SERVO_BAUDRATE=115200)
if dual_arm.servo_buffer(is_scan_servo=True):
    # dual arm and gripper init
    dual_arm.arm_state_init(force_angle_init=False)
    dual_arm.set_all_servo_mode(CONTROL_MODE="CIRCLE")
    wrist_gripper.all_ending_init(motion_group=4)
    rospy.loginfo("-----dual_arm init success!-----")
    dual_arm.logging_arm_info()         # logging servo info 
    dual_arm.get_arm_current_angle()    # logging servo angle info
    time.sleep(1)
else:
    rospy.logerr("ERROR: dual arm and gripper init failed!!!")
    

# global control obj from dual_control_manager
dual_arm_nh = dual_arm 
wrist_gripper = wrist_gripper

def main():
    # ros gripper control node init 
    rospy.init_node(rospy.get_param('/node_info/gripper_control_node_name'), 
                        anonymous=True)
    rospy.loginfo("Gripper Control Node init Success!")
    servo_pub = rospy.Subscriber('/robot_arm_state', RobotState , gripper_control_callback, queue_size = 1)
    rospy.spin()

def gripper_control_callback(RobotState):
    
    # 左臂手腕手爪位置伺服，0（IO5）-》it-3，1(IO6)->it-[8]
    wrist_gripper.set_servo_angle(servo_idx=0, angle=RAD2DEG * RobotState.leftArmPosition[3])
    wrist_gripper.set_servo_angle(servo_idx=1, angle=RAD2DEG * RobotState.leftArmPosition[4])

    # 右臂手腕手爪位置伺服，2（IO12）-》it-7，3(IO13)->it-[9]
    wrist_gripper.set_servo_angle(servo_idx=2, angle=RAD2DEG * RobotState.rightArmPosition[3])
    wrist_gripper.set_servo_angle(servo_idx=3, angle=RAD2DEG * RobotState.rightArmPosition[4])
    # time.sleep(SHORT_TIME_DELAY)
    # rospy.loginfo("Wrist and Gripper control call back success! ")
    # rospy.loginfo('left wrist:  {}  left gripper:   {}'.format(RobotState.leftArmPostion[3],
                                                                    # RobotState.leftArmPostion[4]))


if __name__ == '__main__':
    try:
        main()
    except KeyboardInterrupt:
        rospy.loginfo('Robot Wrist and gripper control node  END')
        rospy.logerr(" Wrist and gripper control exit()")


